//
// Created by zhan_ on 2025/9/22.
//

/**
******************************************************************************
 * @file    param_config.c
 * @brief   参数配置模块实现文件，用于集中管理PID参数等配置
 ******************************************************************************
 */

#include "param_config.h"


// 当前机器人类型
static RobotType_e current_robot_type = ROBOT_TYPE_infantry;

// 标准机器人PID参数配置
static RobotParams_t infantry_robot_params = {
    .chassis_params = {
        .speed_PID = {
            .Kp = 2.0f,
            .Ki = 0.0f,
            .Kd = 0.0f,
            .IntegralLimit = 3000.0f,
            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
            .MaxOut = 12000.0f,
        },
        .current_PID = {
            .Kp = 0.5f,
            .Ki = 0.0f,
            .Kd = 0.0f,
            .IntegralLimit = 3000.0f,
            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
            .MaxOut = 15000.0f,
        },
        .follow_PID = {
            .Kp = 50.0f,
            .Ki = 0.0f,
            .Kd = 0.0f,
            .IntegralLimit = 1000.0f,
            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
            .MaxOut = 8000.0f,
        },
        .motor_type = M3508
    },
    .gimbal_params = {
        .yaw_angle_PID = {
            .Kp = 0.3f,
            .Ki = 0.0f,
            .Kd = 0.0f,
            .DeadBand = 0.1f,
            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
            .IntegralLimit = 5.0f,
            .MaxOut = 20.0f,
        },
        .yaw_speed_PID = {
            .Kp = 6000.0f,
            .Ki = 100.0f,
            .Kd = 0.0f,
            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
            .IntegralLimit = 12000.0f,
            .MaxOut = 25000.0f,
        },
        .pitch_angle_PID = {
            .Kp = 0.5f,
            .Ki = 0.0f,
            .Kd = 0.0f,
            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
            .IntegralLimit = 5.0f,
            .MaxOut = 20.0f,
        },
        .pitch_speed_PID = {
            .Kp = 5000.0f,
            .Ki = 200.0f,
            .Kd = 0.0f,
            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
            .IntegralLimit = 12000.0f,
            .MaxOut = 28000.0f,
        },
        .motor_type = GM6020
    },
    .shoot_params = {
        .friction_speed_PID = {
            .Kp = 20.0f,
            .Ki = 1.0f,
            .Kd = 0.0f,
            .Improve = PID_Integral_Limit,
            .IntegralLimit = 10000.0f,
            .MaxOut = 15000.0f,
        },
        .friction_current_PID = {
            .Kp = 0.7f,
            .Ki = 0.1f,
            .Kd = 0.0f,
            .Improve = PID_Integral_Limit,
            .IntegralLimit = 10000.0f,
            .MaxOut = 15000.0f,
        },
        .friction_motor_type = M3508,

        .loader_angle_PID = {
            .Kp = 60.0f,
            .Ki = 0.0f,
            .Kd = 0.0f,
            .MaxOut = 20000.0f,
        },
        .loader_speed_PID = {
            .Kp = 1.0f,
            .Ki = 0.1f,
            .Kd = 0.0f,
            .Improve = PID_Integral_Limit | PID_ErrorHandle,
            .IntegralLimit = 5000.0f,
            .MaxOut = 10000.0f,
        },
        .loader_current_PID = {
            .Kp = 0.0f,
            .Ki = 0.0f,
            .Kd = 0.0f,
            .Improve = PID_Integral_Limit,
            .IntegralLimit = 5000.0f,
            .MaxOut = 5000.0f,
        },
        .loader_motor_type = M2006,
    }
};



// 当前使用的PID参数指针
static RobotParams_t* current_pid_params = &infantry_robot_params;

/**
 * @brief 初始化参数配置模块
 */
void ParamConfigInit(void)
{
    // 这里可以根据实际硬件ID或者其他方式来确定机器人类型
    // 目前默认使用标准机器人参数
    RobotType_e robot_type = current_robot_type;

    switch (robot_type) {
        case ROBOT_TYPE_infantry:
            current_pid_params = &infantry_robot_params;
            break;
    }
}




/**
 * @brief 获取当前机器人PID参数
 * @return PID参数结构体指针
 */
RobotParams_t* GetRobotPIDParams(void)
{
    return current_pid_params;
}

